#include <arpa/inet.h>  // for inet_pton
#include <assert.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/wait.h>

#include <atomic>
#include <cmath>
#include <condition_variable>
#include <cstdlib>  // 包含随机数函数rand()和srand()
#include <ctime>    // 包含当前时间函数time()
#include <fstream>
#include <iostream>
#include <mutex>
#include <queue>
#include <thread>

#include "gimbal_client.h"
#include "global.h"
#include "nlohmann/json.hpp"
#include "npu.h"
#include "sat.h"
#include "serialPort.h"
#include "shttpd.h"
#include "sat.h"
#include "nlohmann/json.hpp"
#include "gimbal_client.h"
#include "low_altitude_data.cpp"

#include <condition_variable>
#include <atomic>



// 云台客户端
GimbalClient gimbal_client;



// 识别模块
NPU g_npu;

// 手动状态，true 表示手动模式，false 表示自动模式
std::atomic<bool> manual_state{false};

// 侦测配置
std::map<int, SpyConfig> g_spymap;

// 频谱侦测仪器全局数据存储对象
LowAltitudeDataStorage data_storage;

// 侦测信息计数
std::array<int, 18> g_spy_info{};

static void defSetting(shttpd_arg arg){
    char value[20];
    value[0] = '\0';
    const char *host, *request_method, *query_string, *request_uri;

    request_method = shttpd_get_env(arg, "REQUEST_METHOD");
    request_uri = shttpd_get_env(arg, "REQUEST_URI");
    query_string = shttpd_get_env(arg, "QUERY_STRING");

    if (!strcmp(request_method, "POST"))
    {
        /* If not all data is POSTed, wait for the rest */
        if (arg->flags & SHTTPD_MORE_POST_DATA)
        {
            return;
        }

        (void)shttpd_get_var("set_Ti", arg->in.buf, arg->in.len, value, sizeof(value));
    }
    else if (query_string != NULL)
    {
        (void)shttpd_get_var("set_Ti", query_string, (int)strlen(query_string), value,
                             sizeof(value));
    }

    shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

    shttpd_printf(arg, "设置成功");
    arg->flags |= SHTTPD_END_OF_OUTPUT;
}

static void queryinfo(shttpd_arg arg)
{
    char value[5];
    value[0] = '\0';
    const char *host, *request_method, *query_string, *request_uri;

    request_method = shttpd_get_env(arg, "REQUEST_METHOD");
    request_uri = shttpd_get_env(arg, "REQUEST_URI");
    query_string = shttpd_get_env(arg, "QUERY_STRING");

    if (query_string != NULL)
        (void)shttpd_get_var("cmd", query_string, (int)strlen(query_string), value, sizeof(value));

    shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: application/json\r\n\r\n");

    shttpd_printf(arg,
                  "{\"APP_VERSION\": \"%s\",\"GIMBAL_VERSION\": "
                  "\"%s\",\"MODEL_VERSION\": \"%s\"}",
                  APP_VERSION.c_str(), GIMBAL_VERSION.c_str(), MODEL_VERSION.c_str());
    arg->flags |= SHTTPD_END_OF_OUTPUT;
}

static void setPTZ(shttpd_arg arg)
{
    /* integer passed to us */
    char value[20];
    char value1[20];
    float pan = 0.0;
    float tilt = 0.0;
    /* Change the value of integer variable */
    value[0] = '\0';

    const char *host, *request_method, *query_string, *request_uri;

    request_method = shttpd_get_env(arg, "REQUEST_METHOD");
    request_uri = shttpd_get_env(arg, "REQUEST_URI");
    query_string = shttpd_get_env(arg, "QUERY_STRING");

    (void)shttpd_get_var("cmd", query_string, (int)strlen(query_string), value, sizeof(value));
    (void)shttpd_get_var("param", query_string, (int)strlen(query_string), value1, sizeof(value1));

    uint8_t cmd = atoi(value);
    uint8_t param = atoi(value1);

    // std::cout<< "cmd: "<< value << "param: "<< value1 << std::endl;

    switch (cmd)
    {
        case 1:
            gimbal_client.move_up(param);
            break;

        case 2:
            gimbal_client.move_down(param);
            break;

        case 3:
            gimbal_client.move_left(param);
            break;

        case 4:
            gimbal_client.move_right(param);
            break;

        case 5:
            gimbal_client.move_left_top(param);
            break;

        case 6:
            gimbal_client.move_right_top(param);
            break;

        case 7:
            gimbal_client.move_left_down(param);
            break;

        case 8:
            gimbal_client.move_right_down(param);
            break;

        case 9:
            gimbal_client.stop_move();
            break;

        case 10:
            gimbal_client.move_to_preset_point(param);
            break;

        case 11:
            gimbal_client.set_preset_point(param);
            break;

        case 12:
            gimbal_client.set_countermeasure(true);
            break;

        case 13:
            gimbal_client.set_countermeasure(false);
            break;

        case 14:
            gimbal_client.add_zoom_ratio();
            break;

        case 15:
            gimbal_client.sub_zoom_ratio();
            break;

        case 16:
            pan = gimbal_client.get_current_pan();
            tilt = gimbal_client.get_current_tilt();
            break;

        case 17:
            gimbal_client.set_pan_absolute(param);
            break;

        case 18:
            gimbal_client.set_tilt_reltive(param);
            break;
        default:
            break;
    }

    shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

    if (cmd == 16)
        shttpd_printf(arg, "pan:%f,tilt:%f", pan, tilt);
    else
        shttpd_printf(arg, "设置成功");
    arg->flags |= SHTTPD_END_OF_OUTPUT;
}
// 处理 HTTP
// 文件上传请求，接收客户端上传的文件，对文件进行处理（如去除文件头部信息、修改文件权限等）
// 并根据文件名执行不同的更新操作（如机芯固件更新、云台更新）
char *path = NULL;
static void update(shttpd_arg arg)
{
    const char *s;

    struct state
    {
        size_t cl;    /* Content-Length   */
        size_t nread; /* Number of bytes read */
        FILE *fp;
    } *state;

    /* If the connection was broken prematurely, cleanup */
    if (arg->flags & SHTTPD_CONNECTION_ERROR && arg->state)
    {
        (void)fclose(((struct state *)arg->state)->fp);
        free(arg->state);
    }
    else if ((s = shttpd_get_header(arg, "Content-Length")) == NULL)
    {
        shttpd_printf(arg, "HTTP/1.0 411 Length Required\n\n");
        arg->flags |= SHTTPD_END_OF_OUTPUT;
    }
    else if (arg->state == NULL)
    {
        /* New request. Allocate a state structure, and open a file */
        char *file_name_begin = strstr(arg->in.buf, "filename=\"");
        char *file_name_end = strstr(file_name_begin, "\"\r\n");
        file_name_begin += strlen("filename=\"");
        path = (char *)malloc(strlen(file_name_begin) - strlen(file_name_end) + 1);
        memcpy(path, file_name_begin, strlen(file_name_begin) - strlen(file_name_end));
        *(path + strlen(file_name_begin) - strlen(file_name_end) + 1) = '\0';

        std::cout << "filename:\"" << path << "\"\n";

        arg->state = state = static_cast<struct state *>(calloc(1, sizeof(*state)));
        state->cl = strtoul(s, NULL, 10);
        state->fp = fopen(path, "wb+");

        shttpd_printf(arg,
                      "HTTP/1.0 200 OK\r\n"
                      "Content-Type: text/plain\r\n\r\n");
    }
    else
    {
        state = static_cast<struct state *>(arg->state);
        (void)fwrite(arg->in.buf, arg->in.len, 1, state->fp);
        state->nread += arg->in.len;
        arg->in.num_bytes = arg->in.len;

        if (state->nread >= state->cl)
        {
            shttpd_printf(arg, "Written %d bytes to %s", state->nread, path);
            (void)fclose(state->fp);
            int file_size = state->nread;
            free(state);
            state = NULL;

            char *new_path = (char *)malloc(strlen(path) + 4);
            strcpy(new_path, "new_");
            strcat(new_path, path);

            FILE *infile = fopen(path, "rb");
            FILE *outfile = fopen(new_path, "wb+");

            int head_len = 0;

            char *buffer = (char *)malloc(1024);
            fread(buffer, 1024, 1, infile);
            char *head_last = strstr(buffer, "\r\n\r\n");
            head_len = head_last - buffer + 4;

            fclose(infile);

            infile = fopen(path, "rb");
            fread(buffer, head_len, 1, infile);
            int body_len = file_size - head_len - 46;
            std::cout << "body_len" << std::to_string(body_len) << std::endl;
            int i = 0;
            while (i < body_len)
            {
                i += 1024;
                if (i > body_len)
                {
                    std::cout << "i:" << std::to_string(i) << std::endl;
                    int last_size = body_len + 1024 - i;
                    std::cout << "last_size:" << std::to_string(last_size) << std::endl;
                    char *last_buffer = (char *)malloc(last_size);
                    fread(last_buffer, last_size, 1, infile);
                    fwrite(last_buffer, last_size, 1, outfile);
                    free(last_buffer);
                    last_buffer = NULL;

                    break;
                }
                char *line_buffer = (char *)malloc(1024);
                fread(line_buffer, 1024, 1, infile);
                fwrite(line_buffer, 1024, 1, outfile);
                free(line_buffer);
                line_buffer = NULL;
            }

            fclose(infile);
            fclose(outfile);

            remove(path);
            rename(new_path, path);

            char chmod_file[500] = {0};
            strcpy(chmod_file, "chmod 777 ");
            strcat(chmod_file, path);
            system(chmod_file);

            free(buffer);
            buffer = NULL;
            free(new_path);
            new_path = NULL;

            if (!strncmp(path, "M400-IPCS", 9))
            {
                char mv_file[500] = {0};
                strcpy(mv_file, "mv \"");
                strcat(mv_file, path);
                strcat(mv_file, "\" ../");
                std::cout << "cmd:" << mv_file << "\n";
                system(mv_file);

                system("rm -r /date/src");
                system("rm -r /date/models");

                char unzip_file[500] = {0};
                strcpy(unzip_file, "unzip ");
                strcat(unzip_file, "/date/\"");
                strcat(unzip_file, path);
                strcat(unzip_file, "\" -d /date/");
                std::cout << "cmd:" << unzip_file << "\n";
                system(unzip_file);

                char rm_file[500] = {0};
                strcpy(rm_file, "rm ");
                strcat(rm_file, "/date/\"");
                strcat(rm_file, path);
                strcat(rm_file, "\"");
                system(rm_file);

                system("chmod 777 /date/src/uavdetr");

                sleep(5);

                shttpd_printf(arg, "机芯固件更新成功成功,等待30s重启");
                arg->flags |= SHTTPD_END_OF_OUTPUT;

                free(path);
                path = NULL;
                system("reboot");
            }
            else if (!strncmp(path, "M400-PTZ", 8))
            {
                char mv_file[500] = {0};
                strcpy(mv_file, "mv \"");
                strcat(mv_file, path);
                strcat(mv_file, "\" /date/data/ptz_update.zip");
                std::cout << "cmd:" << mv_file << "\n";
                system(mv_file);

                system("unzip -o /date/data/ptz_update.zip  -d /date/data/");
                system("rm /date/data/ptz_update.zip");

                char update_file[500] = {0};
                strcpy(update_file, "/date/data/ptz_update.bin ");
                bool ret = gimbal_client.ptz_update(update_file);
                if (ret)
                {
                    shttpd_printf(arg, "云台更新成功,等待30s云台重启");
                    arg->flags |= SHTTPD_END_OF_OUTPUT;
                    std::this_thread::sleep_for(std::chrono::milliseconds(30 * 1000));
                }
                else
                {
                    shttpd_printf(arg, "云台更新失败");
                    arg->flags |= SHTTPD_END_OF_OUTPUT;
                }

                // system("rm /date/data/ptz_update.bin");

                arg->flags |= SHTTPD_END_OF_OUTPUT;
                free(path);
                path = NULL;
            }
        }
    }
}

static void setManualmode(shttpd_arg arg)
{
    char value[5];
    /* Change the value of integer variable */
    value[0] = '\0';
    const char *host, *request_method, *query_string, *request_uri;

    request_method = shttpd_get_env(arg, "REQUEST_METHOD");
    request_uri = shttpd_get_env(arg, "REQUEST_URI");
    query_string = shttpd_get_env(arg, "QUERY_STRING");

    (void)shttpd_get_var("cmd", query_string, (int)strlen(query_string), value, sizeof(value));
    int cmd = atoi(value);

    if (cmd == 1)
    {
        manual_state = true;
        std::cout << "open manual mode." << std::endl;
    }
    else
    {
        std::cout << "close manual mode." << std::endl;
        manual_state = false;
    }

    shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

    shttpd_printf(arg, "设置成功");
    arg->flags |= SHTTPD_END_OF_OUTPUT;
}



// static void autoDetConf(shttpd_arg arg) {
//     using json = nlohmann::json;
//     char value[5];
//     float pan = 0.0;
//     float tilt = 0.0;
//     int zoom = 25; // 默认 zoom 值

//     // 解析 HTTP 请求参数
//     const char *query_string = shttpd_get_env(arg, "QUERY_STRING");
//     shttpd_get_var("num", query_string, (int)strlen(query_string), value, sizeof(value));
//     int num = atoi(value);

//     // 1. 读取现有的 JSON 文件
//     std::ifstream in_file("../data/spy_config.json");
//     json config_data;
//     if (in_file.good()) {
//         in_file >> config_data;
//     } else {
//         // 如果文件不存在，初始化一个空数组
//         config_data = json::array();
//     }
//     in_file.close();

//     if (num > 10 && num < 19) {
//         // 2. 更新指定 id 的配置项
//         pan = gimbal_client.get_current_pan();
//         tilt = gimbal_client.get_current_tilt();

// #ifdef USE_12MM_LENS
//         zoom = (num == 18 || num >= 14) ? 25 : 26;
// #else
//         zoom = 25;
// #endif

//         // 查找或创建对应 id 的配置项
//         bool found = false;
//         for (auto &item : config_data) {
//             if (item["id"] == num) {
//                 item["pan"] = pan;
//                 item["tilt"] = tilt;
//                 item["zoom"] = zoom;
//                 found = true;
//                 break;
//             }
//         }

//         if (!found) {
//             config_data.push_back({
//                 {"id", num},
//                 {"threshold", 2}, // 默认阈值
//                 {"pan", pan},
//                 {"tilt", tilt},
//                 {"zoom", zoom}
//             });
//         }

//         // 3. 返回成功响应
//         shttpd_printf(arg, "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");
//         shttpd_printf(arg, "配置 %d 更新成功: pan=%.2f, tilt=%.2f, zoom=%d", num, pan, tilt, zoom);
//     } else {
//         // 4. 保存全部配置到文件
//         std::ofstream out_file("../data/spy_config.json");
//         if (!out_file) {
//             shttpd_printf(arg, "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");
//             shttpd_printf(arg, "保存文件失败");
//             arg->flags |= SHTTPD_END_OF_OUTPUT;
//             return;
//         }

//         out_file << config_data.dump(4); // 格式化输出，缩进为 4 空格
//         out_file.close();

//         // 5. 返回响应并重启设备
//         shttpd_printf(arg, "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");
//         shttpd_printf(arg, "全部配置保存成功，正在重启设备");
//         arg->flags |= SHTTPD_END_OF_OUTPUT;

//         // 延迟 1 秒后重启
//         sleep(1);
//         system("/sbin/reboot");
//     }

//     arg->flags |= SHTTPD_END_OF_OUTPUT;
// }

static void autoDetConf(shttpd_arg arg){
    /* integer passed to us */
    // int *p = reinterpret_cast<int*>(arg->user_data);
    char value[5];
    float pan = 0.0;
    float tilt = 0.0;

    std::ofstream outfile;
    /* Change the value of integer variable */
    value[0] = '\0';

    const char *host, *request_method, *query_string, *request_uri;

    request_method = shttpd_get_env(arg, "REQUEST_METHOD");
    request_uri = shttpd_get_env(arg, "REQUEST_URI");
    query_string = shttpd_get_env(arg, "QUERY_STRING");

    (void)shttpd_get_var("num", query_string, (int)strlen(query_string), value, sizeof(value));
    int num = atoi(value);

    std::cout << "Setting num : " << num << std::endl;

    if (num > 10 && num < 19)
    {
        memset(save_conf[num - 11], 0x00, 30);
        save_conf[num - 11][0] = '\0';

        pan = gimbal_client.get_current_pan();
        tilt = gimbal_client.get_current_tilt();

        std::string pan_str = std::to_string(pan);
        std::string tilt_str = std::to_string(tilt);

        strcat(save_conf[num - 11], value);
        strcat(save_conf[num - 11], ",");
        strcat(save_conf[num - 11], pan_str.c_str());
        strcat(save_conf[num - 11], ",");
        strcat(save_conf[num - 11], tilt_str.c_str());

#ifdef USE_12MM_LENS
        if (num == 18)
            strcat(save_conf[num - 11], ",25");
        else if (num < 14)
            strcat(save_conf[num - 11], ",26\n");
        else
            strcat(save_conf[num - 11], ",25\n");
#else
        if (num != 18)
            strcat(save_conf[num - 11], ",25\n");
        else
            strcat(save_conf[num - 11], ",25");
#endif

        std::cout << "save_conf[" << num - 11 << "]:" << save_conf[num - 11] << std::endl;

        shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

        shttpd_printf(arg, "设置成功");
        arg->flags |= SHTTPD_END_OF_OUTPUT;
    }
    else
    {
        // 创建ofstream对象并打开文件
        outfile.open("../data/det.conf");
        if (!outfile)
        {
            std::cerr << "无法打开文件 " << std::endl;
            shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

            shttpd_printf(arg, "设置失败，请重新设置");
            arg->flags |= SHTTPD_END_OF_OUTPUT;
        }
        else
        {
            char save_conf_all[300];
            save_conf_all[0] = '\0';
            for (uint8_t i = 0; i < 8; i++) strcat(save_conf_all, save_conf[i]);

            outfile << save_conf_all;
            outfile.close();

            shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

            shttpd_printf(arg, "全部设置成功，正在重启设备");
            arg->flags |= SHTTPD_END_OF_OUTPUT;
        }

        int result = system("reboot");
        if (result == -1)
        {
            perror("system failed");

            shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

            shttpd_printf(arg, "重启失败");
            arg->flags |= SHTTPD_END_OF_OUTPUT;
        }
        else if (WIFEXITED(result))
        {
            int status = WEXITSTATUS(result);
            if (status != 0)
            {
                std::cerr << "reboot command exited with status " << status << std::endl;

                shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

                shttpd_printf(arg, "重启失败");
                arg->flags |= SHTTPD_END_OF_OUTPUT;
            }
        }
        else
        {
            std::cerr << "reboot command did not exit normally" << std::endl;

            shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

            shttpd_printf(arg, "重启失败");
            arg->flags |= SHTTPD_END_OF_OUTPUT;
        }
        // 如果成功，system函数不会立即返回，但程序会在这里继续执行（实际上不会）

        shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

        shttpd_printf(arg, "正在重启");
        arg->flags |= SHTTPD_END_OF_OUTPUT;
    }
}


static void setWP(shttpd_arg arg){
    /* integer passed to us */
    // int *p = reinterpret_cast<int*>(arg->user_data);
    char value[5];
    char value1[5];
    /* Change the value of integer variable */
    value[0] = '\0';
    static char save_conf[200];

    const char *host, *request_method, *query_string, *request_uri;

    request_method = shttpd_get_env(arg, "REQUEST_METHOD");
    request_uri = shttpd_get_env(arg, "REQUEST_URI");
    query_string = shttpd_get_env(arg, "QUERY_STRING");

    (void)shttpd_get_var("cmd", query_string, (int)strlen(query_string), value, sizeof(value));
    (void)shttpd_get_var("id", query_string, (int)strlen(query_string), value1, sizeof(value1));
    int cmd = atoi(value);
    int id = atoi(value1);

    if (cmd == 1)
        counter_action |= (id & 0xFF);
    else
        counter_action &= ~(id & 0xFF);

    shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

    shttpd_printf(arg, "设置成功");
    arg->flags |= SHTTPD_END_OF_OUTPUT;
}

static void restart(shttpd_arg arg)
{
    /* integer passed to us */
    // int *p = reinterpret_cast<int*>(arg->user_data);
    char value[5];
    /* Change the value of integer variable */
    value[0] = '\0';
    static char save_conf[200];

    const char *host, *request_method, *query_string, *request_uri;

    request_method = shttpd_get_env(arg, "REQUEST_METHOD");
    request_uri = shttpd_get_env(arg, "REQUEST_URI");
    query_string = shttpd_get_env(arg, "QUERY_STRING");

    int result = system("reboot");
    if (result == -1)
    {
        perror("system failed");

        shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

        shttpd_printf(arg, "重启失败");
        arg->flags |= SHTTPD_END_OF_OUTPUT;
    }
    else if (WIFEXITED(result))
    {
        int status = WEXITSTATUS(result);
        if (status != 0)
        {
            std::cerr << "reboot command exited with status " << status << std::endl;

            shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

            shttpd_printf(arg, "重启失败");
            arg->flags |= SHTTPD_END_OF_OUTPUT;
        }
    }
    else
    {
        std::cerr << "reboot command did not exit normally" << std::endl;

        shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

        shttpd_printf(arg, "重启失败");
        arg->flags |= SHTTPD_END_OF_OUTPUT;
    }
    // 如果成功，system函数不会立即返回，但程序会在这里继续执行（实际上不会）

    shttpd_printf(arg, "%s", "HTTP/1.0 200 OK\r\nContent-Type: text/plain\r\n\r\n");

    shttpd_printf(arg, "正在重启");
    arg->flags |= SHTTPD_END_OF_OUTPUT;
}

void web_thread()
{
#define ALIAS_DIR "../data/"
#define ALIAS_URI "download"
    int data = -15;
    shttpd_ctx_t *ctx = shttpd_init(0, 0);
    shttpd_set_option(ctx, "ports", "8088");
    shttpd_set_option(ctx, "aliases", ALIAS_URI "=" ALIAS_DIR);
    shttpd_set_option(ctx, "index_files", "../data/index.html");
    shttpd_register_uri(ctx, "/update", &update, NULL);
    shttpd_register_uri(ctx, "/defSetting", &defSetting, (void *)&data);
    shttpd_register_uri(ctx, "/queryinfo", &queryinfo, (void *)&data);
    shttpd_register_uri(ctx, "/setPTZ", &setPTZ, (void *)&data);
    shttpd_register_uri(ctx, "/setWP", &setWP, (void *)&data);
    shttpd_register_uri(ctx, "/autoDetConf", &autoDetConf, (void *)&data);
    shttpd_register_uri(ctx, "/setManualmode", &setManualmode, (void *)&data);
    shttpd_register_uri(ctx, "/restart", &restart, (void *)&data);

    while (true)
    {
        shttpd_poll(ctx, 10000);
    }
    shttpd_fini(ctx);
}

void rgn_operate(RGN &rgn_FS_1, RGN &rgn_FS_2, RGN &rgn_FS_3, RGN &rgn_FS_4)
{
    rgn_FS_1.Init(0, 21, HI_RGN_LINEEX);
    rgn_FS_2.Init(0, 22, HI_RGN_LINEEX);
    rgn_FS_3.Init(0, 23, HI_RGN_LINEEX);
    rgn_FS_4.Init(0, 24, HI_RGN_LINEEX);

    std::vector<Vector> vertices_1 = {Vector(1920, 1720), Vector(1920, 1130)};
    std::vector<Vector> vertices_3 = {Vector(1920, 1030), Vector(1920, 440)};

    std::vector<Vector> vertices_2 = {Vector(2560, 1080), Vector(1990, 1080)};
    std::vector<Vector> vertices_4 = {Vector(1870, 1080), Vector(1280, 1080)};

    rgn_FS_1.set_lineex_chn_attr(vertices_1, 0xFFFFFF);
    rgn_FS_3.set_lineex_chn_attr(vertices_3, 0xFFFFFF);
    rgn_FS_2.set_lineex_chn_attr(vertices_2, 0xFFFFFF);
    rgn_FS_4.set_lineex_chn_attr(vertices_4, 0xFFFFFF);

    rgn_FS_1.set_lineex_chn_attr(vertices_1,0xFFFFFF);
    rgn_FS_3.set_lineex_chn_attr(vertices_3,0xFFFFFF);
    rgn_FS_2.set_lineex_chn_attr(vertices_2,0xFFFFFF);  
    rgn_FS_4.set_lineex_chn_attr(vertices_4,0xFFFFFF); 
    
    // custom_printf("attach region handle xx to channel 0");
    rgn_FS_1.attach_to_chn(); 
    rgn_FS_3.attach_to_chn();
    rgn_FS_2.attach_to_chn();  
    rgn_FS_4.attach_to_chn(); 
    // custom_printf("attach region handle xx");
}

void rgn_clear(RGN &rgn_FS_1, RGN &rgn_FS_2, RGN &rgn_FS_3, RGN &rgn_FS_4, RGN &rgn_FS_CORNER_RECT)
{
    rgn_FS_CORNER_RECT.detach_from_chn();
    rgn_FS_CORNER_RECT.destroy_region();

    rgn_FS_1.detach_from_chn();
    rgn_FS_1.destroy_region();

    rgn_FS_3.detach_from_chn();
    rgn_FS_3.destroy_region();

    rgn_FS_2.detach_from_chn();
    rgn_FS_2.destroy_region();

    rgn_FS_4.detach_from_chn();
    rgn_FS_4.destroy_region();
}

void send_target_status(int id, const char *status, int w, int h, int cmd)
{
    gimbal_client.send_target_to_box(id, status, w, h, cmd, 1);
    // if (!gimbal_client.send_target_to_box(id, status, w, h, cmd, 1)) {
    //     custom_printf("send %d failed", cmd);
    // } else {
    //     custom_printf("send %d done", cmd);
    // }
}

// 接收来自侦测相机的消息
int init_server_socket()
{
    int fd = socket(PF_INET, SOCK_DGRAM, 0);
    if (fd == -1)
    {
        printf("Create socket failed!!\n");
        return -1;
    }
    struct sockaddr_in addr;
    bzero(&addr, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_port = htons(UDP_SERVER_PORT);
    addr.sin_addr.s_addr = htonl(INADDR_ANY);  // 接收任意IP发来的数据
    /* 绑定socket */
    if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0)
    {
        perror("connect");
        exit(1);
    }
    struct timeval timeout;
    timeout.tv_sec = 0;
    timeout.tv_usec = 5;
    if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) == -1)
    {
        printf("setsockopt failed!!\n");
        return -1;
    }
    return fd;
}

SpyMessage recv_msg_from_spy(int fd) {
    // custom_printf("Waiting for spy message.");
    char recvbuf[128] = {0};
    struct sockaddr_in peer_addr;
    int len = sizeof(peer_addr);
    int nrecv = recvfrom(fd, recvbuf, sizeof(recvbuf), 0, (struct sockaddr *)&peer_addr, (socklen_t *)&len);
    SpyMessage msg;
    msg.spy_id = (peer_addr.sin_addr.s_addr >> 24) & 0xFF;
    if (nrecv == 10 && recvbuf[8] == 0x0D && recvbuf[9] == 0x0A)
    {
         // 从接收缓冲区提取数据
        int width = recvbuf[4] * 256 + recvbuf[5];

        if (width == 0) {
            // 无目标信息，更新跟踪状态
            g_spy_info[msg.spy_id] = std::max(0, g_spy_info[msg.spy_id] - 1);
            custom_printf("nrecv from spy %d: %d, %d, %d, %d", msg.spy_id, msg.bbox.x, msg.bbox.y, msg.bbox.w, msg.bbox.h);
        } else {
            // 有目标信息，更新跟踪状态
            g_spy_info[msg.spy_id]++; // 有目标信息，更新跟踪状态
        }

        // 如果跟踪状态达到阈值，提取目标信息
        if (g_spy_info[msg.spy_id] >= MAX_TRACK_COUNT) {
            msg.bbox.x = recvbuf[0] * 256 + recvbuf[1];
            msg.bbox.y = recvbuf[2] * 256 + recvbuf[3];
            msg.bbox.w = recvbuf[4] * 256 + recvbuf[5];
            msg.bbox.h = recvbuf[6] * 256 + recvbuf[7];
            custom_printf("nrecv from spy %d: %d, %d, %d, %d", msg.spy_id, msg.bbox.x, msg.bbox.y, msg.bbox.w, msg.bbox.h);
            g_spy_info[msg.spy_id] = 0; // 重置跟踪状态
        }
    } else {
        custom_printf("Error from spy. nrecv %d\n", nrecv);
    }
    return msg;
}

// 接收来自频谱侦测仪器的数据
int init_tcp_server_socket(int port) {
    int fd = socket(AF_INET, SOCK_STREAM, 0);
    if (fd == -1) {
        perror("Create TCP socket failed");
        return -1;
    }

    // 设置SO_REUSEADDR避免重启时地址占用问题
    int opt = 1;
    if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) {
        perror("setsockopt SO_REUSEADDR failed");
        close(fd);
        return -1;
    }

    struct sockaddr_in addr;
    bzero(&addr, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_port = htons(port);
    addr.sin_addr.s_addr = htonl(INADDR_ANY);

    if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        perror("TCP bind failed");
        close(fd);
        return -1;
    }

    if (listen(fd, SOMAXCONN) < 0) {
        perror("TCP listen failed");
        close(fd);
        return -1;
    }

    return fd;
}

ssize_t read_n(int fd, char* buf, size_t n) {
    size_t total = 0;
    while (total < n) {
        ssize_t r = recv(fd, buf + total, n - total, 0);
        if (r < 0) {
            if (errno == EINTR) continue;  // 被中断，重试
            perror("recv error");
            return -1;
        }
        if (r == 0) {
            // 对端关闭
            return total;
        }
        total += r;
    }
    return total;
}

const int MAX_JSON_SIZE = 65536;

bool parse_tcp_packet(int conn_fd, nlohmann::json& json_data) {
    char header_buf[12];
    if (read_n(conn_fd, header_buf, sizeof(header_buf)) != sizeof(header_buf)) {
        perror("Failed to read packet header");
        return false;
    }

    // uint32_t head, type, length;
    // memcpy(&head, header_buf, 4); head = ntohl(head);
    // memcpy(&type, header_buf + 4, 4); type = ntohl(type);
    // memcpy(&length, header_buf + 8, 4); length = ntohl(length);

    uint32_t head, type, length;
    memcpy(&head, header_buf, 4);
    memcpy(&type, header_buf + 4, 4);
    memcpy(&length, header_buf + 8, 4);


    if (head != 0x55555555) {
        std::cerr << "Invalid packet head: 0x" << std::hex << head << std::endl;
        return false;
    }

    int json_len = length - 16;
    if (json_len <= 0 || json_len > MAX_JSON_SIZE) {
        std::cerr << "Invalid or too large JSON length: " << json_len << std::endl;
        return false;
    }

    std::vector<char> json_buf(json_len + 1);
    if (read_n(conn_fd, json_buf.data(), json_len) != json_len) {
        perror("Failed to read JSON data");
        return false;
    }
    json_buf[json_len] = '\0';

    uint32_t tail;
    if (read_n(conn_fd, reinterpret_cast<char*>(&tail), sizeof(tail)) != sizeof(tail)) {
        perror("Failed to read packet tail");
        return false;
    }
    tail = ntohl(tail);

    if (tail != 0xaaaaaaaa) {
        std::cerr << "Invalid packet tail: 0x" << std::hex << tail << std::endl;
        return false;
    }

    try {
        json_data = nlohmann::json::parse(json_buf.data());
    } catch (const std::exception& e) {
        std::cerr << "Failed to parse JSON data: " << e.what() << std::endl;
        return false;
    }

    return true;
}


// 存储站点和无人机信息
void handle_client_connection(int conn_fd) {
    while (true) {
        nlohmann::json json_data;
        if (!parse_tcp_packet(conn_fd, json_data)) {
            break;
        }

        // 根据数据类型存储
        if (json_data.contains("station_id")) {
            data_storage.storeStationData(json_data);
            // std::cout << "Stored station data" << std::endl;
        } else if (json_data.contains("station_droneInfo")) {
            data_storage.storeDroneData(json_data);
            // std::cout << "Stored drone data" << std::endl;
        }
    }
    close(conn_fd);
}

// tcp线程回调函数
void tcp_server_thread(int port) {
    int tcp_fd = init_tcp_server_socket(port);
    if (tcp_fd == -1) {
        return;
    }

    std::cout << "TCP server started on port " << port << std::endl;

    while (true) {
        struct sockaddr_in client_addr;
        socklen_t client_len = sizeof(client_addr);
        
        int conn_fd = accept(tcp_fd, (struct sockaddr *)&client_addr, &client_len);
        if (conn_fd < 0) {
            perror("TCP accept failed");
            continue;
        }

        char client_ip[INET_ADDRSTRLEN];
        inet_ntop(AF_INET, &client_addr.sin_addr, client_ip, sizeof(client_ip));
        std::cout << "New connection from " << client_ip << ":" << ntohs(client_addr.sin_port) << std::endl;

        // 为每个客户端连接创建新线程处理
        std::thread(handle_client_connection, conn_fd).detach();
    }

    close(tcp_fd);
}

bool recognize(bbox_t& bbox, float score_thresh, int rec_times = 1) {
    for (int i = 0; i < rec_times; i++) {
        if (g_npu.NPUInvoke(bbox, score_thresh)) {
            return true;
        }
    }
    return false;
}

int track_phase1(int timeout, SpyMessage& msg) {
    
    // gimbal_client.set_zoom_ratio(2);    // 初始倍率为1，获得更大视野

    custom_printf("Enter phase 1.");
    int exit_code = -1;
    time_t phase1_start_time = time(NULL);
    time_t lost_start_time = 0;
    time_t now = time(NULL);
    do {
        if (manual_state) {
            // custom_printf("In track phase1, user set manual mode. exit.");
            exit_code = 0;  // 手动模式退出
            break;
        }

        bbox_t phase1_bbox;
        if (!recognize(phase1_bbox, RECOGINZE_SCORE_THRESHOLD, RECOGINZE_COUNT)) { // 阈值为0.5，最多识别次数为5次
            exit_code = 1; // 无目标退出
            break;
        } 

        // custom_printf("Phase1 Open countermeasure");
        gimbal_client.set_countermeasure(true);

        int current_zoom = gimbal_client.get_current_zoom();
        gimbal_client.lpp_control_step_pixel(phase1_bbox,current_zoom); //
        // gimbal_client.lpp_control_step_pixel(phase1_bbox.cx, phase1_bbox.cy);
        send_target_status(100, "locked", phase1_bbox.w, phase1_bbox.h, 3); 
        
        

        if((time(NULL) - phase1_start_time) > timeout) {
            // custom_printf("Phase1 time out, measure counter failed");  
            send_target_status(msg.spy_id, "failed", phase1_bbox.w, phase1_bbox.h, 0);
            std::cout << "超时退出！！！！！"  << std::endl;
            exit_code = 2; // 超时退出
            break;
        }
    } while (true);

    if (exit_code == 1)
    {
        send_target_status(100, "success", 100, 100, 1);
        // custom_printf("phase 1 countermeasure success. send 1");
    }
    else if (exit_code == 2)
    {
        send_target_status(100, "failed", 100, 100, 0);
        // custom_printf("phase 1  countermeasure failed. send 0");
    }
    else
    {
        // custom_printf("phase 1 meet manual mode. exit code %d", exit_code);
    }
    return exit_code;
}

void track_phase2()
{
    custom_printf("Enter phase 2.");
    std::cout << "进入跟踪阶段2!!!!" << std::endl;
    int exit_code = -1;
    do
    {
        if (manual_state)
        {
            custom_printf("Manual mode, track end");
            exit_code = 0;  // 手动模式退出
            break;
        }
        bbox_t phase2_bbox;
        bool ret = recognize(phase2_bbox, RECOGINZE_SCORE_THRESHOLD, RECOGINZE_COUNT);
        if (!ret)
        {
            exit_code = 1;  // 无目标退出
            break;
        }
        send_target_status(RECOGINZE_ID, "locked", phase2_bbox.w, phase2_bbox.h, 3);
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    } while (true);

    if (exit_code == 1)
    {
        send_target_status(RECOGINZE_ID, "success", 0, 0, 4);
        custom_printf("Target vanished. send 4");
    }
}

void spy_message_to_ptz(SpyMessage &msg, double &target_pan, double &target_tilt, int &target_zoom)
{
    double dx = msg.bbox.cx - 0.5;
    double dy = 0.5 - msg.bbox.cy;

    int id = msg.spy_id;
    target_zoom = g_spymap[id].zoom - 1;  // 倍率

    float d_pan = dx * hfov_arr[target_zoom]; // 水平角度 
    float d_tilt = dy * vfov_arr[target_zoom]; // 垂直角度
    
    
    if (id == RECOGINZE_ID) {
        target_pan = gimbal_client.get_current_pan() + d_pan;
        target_tilt = gimbal_client.get_current_tilt() + d_tilt;
    }
    else
    {
        target_pan = g_spymap[id].pan + d_pan;
        target_tilt = g_spymap[id].tilt + d_tilt;
    }

    
    // custom_printf("spy_id: %d, dx: %f, dy: %f, d_pan: %f, d_tilt: %f, target_pan: %f, target_tilt: %f, target_zoom: %d", id, dx, dy, d_pan, d_tilt, target_pan, target_tilt, target_zoom);
    return;
}

int main(int argc, char *argv[])
{
    bool yvu_model = true;
    if (argc == 2)
    {
        if (strcmp(argv[1], "bgr") == 0)
        {
            yvu_model = false;
        }
    }
    
    int fd = init_server_socket();
    std::thread server_thread(tcp_server_thread, TCP_SERVER_PORT);
    server_thread.detach();
    g_spymap = load_spy_config("../data/spy_config.json");
    // custom_printf("spymap size: %d", g_spymap.size());
    gimbal_client.gimbal_init();
    std::thread WebThread(web_thread);
    WebThread.detach();

    std::string model_file = "../models/" + MODEL_VERSION + ".om";
    if (!g_npu.Init(model_file, yvu_model))
    {
        custom_printf("Init model failed.");
        return -1;
    }

    RGN rgn_FS_1, rgn_FS_2, rgn_FS_3, rgn_FS_4, rgn_FS_CORNER_RECT;
    bbox_t FS_box = {1668, 828, 504, 504, 0, 0, 0, 0};
    custom_printf("attach region handle 5 to channel 0");
    rgn_FS_CORNER_RECT.Init(0, 5, HI_RGN_CORNER_RECTEX);  // HI_RGN_CORNER_RECTEX
    rgn_FS_CORNER_RECT.set_attr(FS_box, 0xFFFFFC);  //0xFFFFFC
    rgn_FS_CORNER_RECT.attach_to_chn(); 
    custom_printf("attach done.");
    
    rgn_operate(rgn_FS_1, rgn_FS_2, rgn_FS_3, rgn_FS_4); 

    auto cleanup = [&]() { rgn_clear(rgn_FS_1, rgn_FS_2, rgn_FS_3, rgn_FS_4, rgn_FS_CORNER_RECT); };

    gimbal_client.set_zoom_ratio(2);    // 初始倍率为1
    while (true)
    {
        bbox_t bbox_tmp;
        SpyMessage msg = recv_msg_from_spy(fd);

        // 获取到频谱侦测仪器的消息
        auto latest_station = data_storage.getLatestStationData();  // 获取全部站点信息
        if (!latest_station.empty()) {
            std::cout << "Latest station: " << latest_station.dump() << std::endl;
        }
        
        auto latest_drone = data_storage.getLatestDroneData();  // 获取全部无人机信息
        if (!latest_drone.empty()) {
            std::cout << "Latest drone: " << latest_drone.dump() << std::endl;
        }

        // 当侦测相机、识别相机、频谱侦测都没有发现目标，继续下一个循环
        bool has_spectrum_target = !latest_station.empty() || !latest_drone.empty();
        if (!recognize(bbox_tmp, SPY_SCORE_THRESHOLD) && msg.bbox.w == 0) {
            continue;
        }
        if (bbox_tmp.w > 0)
        {  // 机芯优先级高, 机芯id固定为10
            msg.spy_id = RECOGINZE_ID;
            msg.bbox = bbox_tmp;
        }
        send_target_status(msg.spy_id, "detected", bbox_tmp.w, bbox_tmp.h, 2);
        
        // 手动模式
        if (manual_state)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            continue;
        }
        //  接受侦测消息

        double taget_pan = 0, target_tilt = 0; int target_zoom = 0;
        spy_message_to_ptz(msg, taget_pan, target_tilt, target_zoom);
        // x5 -> x2
        gimbal_client.lpp_control_to_destination(taget_pan, target_tilt, target_zoom, 100, 2.0);
        
        
        
        // 跟踪逻辑：
        // 在跟踪期间，只要识别到目标即刻发送目标信息给盒子。

        // 反制状态码说明：
        // - 第一阶段（状态1）：M400跟踪反制状态：
        // M400反制失败，发送反制失败通知。
        // M400反制成功，发送反制成功通知 ：规定时长内，无人机离开警戒范围。
        // - 第二阶段（状态2）：M400转纯跟踪状态：
        // 无人机离开警戒范围， 发送跟踪结束通知
        if (track_phase1(TRACK_PHASE1_TIMEOUT_SECOND, msg) == 2)
        {  // phase1 超时退出, 进入phase2
            custom_printf("Phase1 time out, Close countermeasure");
            gimbal_client.set_countermeasure(false);
            track_phase2();
        }
        gimbal_client.set_countermeasure(false);
    }

    // cleanup();
    return 0;
}